//备注：拷贝代码请加上作者信息
//作者：王海涛
//邮箱：1126471088@qq.com
//版本：V1.0.0
#include "pid_float.h"
#include <string.h>
#include <math.h>

/********************* 自适应调参核心函数（静态） *********************/
static void WHT_PID_Float_Adaptive_Tune(WHT_PID_Float_Ctrl_t *pid)
{
    if (pid == NULL || !pid->Adaptive_En) return;
    
    // 1. 计算误差绝对值、误差变化率
    float err_abs = fabsf(pid->Error);             // 误差绝对值
    float err_diff = pid->Error - pid->Error_Prev; // 误差变化率
    float err_diff_abs = fabsf(err_diff);          // 变化率绝对值

    // 2. 自适应调参规则
    if (err_abs > pid->Adaptive_Err_Large)
    {
        // 场景1：误差大→优先快速跟踪，抑制超调
        pid->kp = pid->kp_max;  // 最大kp：快速拉近误差
        pid->ki = pid->ki_min;  // 最小ki：避免积分饱和
        pid->kd = pid->kd_min;  // 最小kd：减少振荡
    }
    else if (err_abs > pid->Adaptive_Err_Med)
    {
        // 场景2：误差中→平衡响应速度和稳定性
        pid->kp = (pid->kp_max + pid->kp_min) / 2.0f;          // kp=平均值
        pid->ki = fminf(pid->ki_min * 2.0f, pid->ki_max);      // ki=2*最小值（不超过max，用fminf安全限制）
        pid->kd = (pid->kd_min + (pid->kd_max / 2.0f)) / 2.0f; // kd=（min + max/2）/2
    }
    else
    {
        // 场景3：误差小→优先稳定和消静差
        if (err_diff_abs < pid->Adaptive_Diff_Small)
        {
            // 变化慢→增大ki消静差，减小kp防振荡
            pid->kp = fminf(pid->kp_min * 2.0f, pid->kp_max); // kp=2*最小值（不超过max）
            pid->ki = pid->ki_max;                            // 最大ki：快速消静差
            pid->kd = pid->kd_min;                            // 最小kd：不影响稳定
        }
        else
        {
            // 变化快→增大kd抑制振荡，减小ki防饱和
            pid->kp = (pid->kp_min + (pid->kp_max / 2.0f)) / 2.0f; // kp=中等值
            pid->ki = fmaxf(pid->ki_max / 2.0f, pid->ki_min);      // ki=max/2（不低于min，用fmaxf安全限制）
            pid->kd = pid->kd_max;                                 // 最大kd：抑制振荡
        }
    }
}

/**
 * @brief  PID初始化函数
 */
static void WHT_PID_Float_Init(WHT_PID_Float_Ctrl_t *pid, float kp, float ki, float kd, float output_min, float output_max)
{
    if (pid == NULL) return;
    
    // 初始化PID系数
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    
    // 初始化设定值和反馈值（默认0.0f）
    pid->Setpoint = 0.0f;
    pid->Feedback = 0.0f;
    pid->Error = 0.0f;
    pid->Error_Prev = 0.0f;
    pid->Error_Sum = 0.0f;
    
    // 初始化输出限幅
    pid->Output_Min = output_min;
    pid->Output_Max = output_max;
    
    // 初始化积分限幅（默认禁用，范围[-2.0f, 2.0f]，可后续配置）
    pid->Integral_Limit_En = false;
    pid->Integral_Min = -2.0f;
    pid->Integral_Max = 2.0f;
    
    // 初始化微分滤波（默认禁用，系数0.9f）
    pid->Derivative_Filter_En = false;
    pid->Derivative_Alpha = 0.9f;
    pid->Derivative_Filtered = 0.0f;
    
    // 初始化自适应参数（默认禁用，默认范围）
    pid->Adaptive_En = false;
    pid->kp_min = 0.1f;
    pid->kp_max = 1.0f;
    pid->ki_min = 0.01f;
    pid->ki_max = 0.5f;
    pid->kd_min = 0.05f;
    pid->kd_max = 0.8f;
    // 默认误差阈值
    pid->Adaptive_Err_Large = 0.125f;
    pid->Adaptive_Err_Med = 0.0625f;
    pid->Adaptive_Diff_Small = 0.03125f;
}

/**
 * @brief  配置积分限幅
 */
static void WHT_PID_Float_Config_Integral_Limit(WHT_PID_Float_Ctrl_t *pid, bool enable, float integral_min, float integral_max)
{
    if (pid == NULL) return;
    
    pid->Integral_Limit_En = enable;
    pid->Integral_Min = integral_min;
    pid->Integral_Max = integral_max;
    
    // 立即对当前积分值进行限幅
    if (enable)
    {
        if (pid->Error_Sum > integral_max)      pid->Error_Sum = integral_max;
        else if (pid->Error_Sum < integral_min) pid->Error_Sum = integral_min;
    }
}

/**
 * @brief  配置微分滤波
 */
static void WHT_PID_Float_Config_Derivative_Filter(WHT_PID_Float_Ctrl_t *pid, bool enable, float alpha)
{
    if (pid == NULL) return;
    
    pid->Derivative_Filter_En = enable;
    // 确保滤波系数在合理范围（0~1）
    if (alpha < 0.0f)      pid->Derivative_Alpha = 0.0f;
    else if (alpha > 1.0f) pid->Derivative_Alpha = 1.0f;
    else                   pid->Derivative_Alpha = alpha;
}

/**
 * @brief  设置PID设定值
 */
static void WHT_PID_Float_Set_Setpoint(WHT_PID_Float_Ctrl_t *pid, float setpoint)
{
    if (pid == NULL) return;
    pid->Setpoint = setpoint;
}

/**
 * @brief  PID计算核心函数
 */
static float WHT_PID_Float_Compute(WHT_PID_Float_Ctrl_t *pid, float feedback)
{
    if (pid == NULL) return 0.0f;
    
    float p_out, i_out, d_out, total_out;
    
    // 1. 更新反馈值和当前误差
    pid->Feedback = feedback;
    pid->Error = pid->Setpoint - feedback;  // 误差 = 设定值 - 反馈值

    /********************* 自适应调参（启用时执行） *********************/
    WHT_PID_Float_Adaptive_Tune(pid);

    // 2. 比例项计算
    p_out = pid->kp * pid->Error;
    
    // 3. 积分项计算
    pid->Error_Sum += pid->Error;  // 积分累计
    // 积分限幅（启用时执行）
    if (pid->Integral_Limit_En)
    {
        if (pid->Error_Sum > pid->Integral_Max)      pid->Error_Sum = pid->Integral_Max;
        else if (pid->Error_Sum < pid->Integral_Min) pid->Error_Sum = pid->Integral_Min;
    }
    // 积分项输出
    i_out = pid->ki * pid->Error_Sum;
    
    // 4. 微分项计算
    float derivative = pid->Error - pid->Error_Prev;  // 误差变化率
    // 微分滤波（一阶低通）
    if (pid->Derivative_Filter_En)
    {
        // 滤波公式：filtered = alpha*current + (1-alpha)*previous
        pid->Derivative_Filtered = pid->Derivative_Alpha * derivative + (1.0f - pid->Derivative_Alpha) * pid->Derivative_Filtered;
        derivative = pid->Derivative_Filtered;
    }
    // 微分项输出
    d_out = pid->kd * derivative;
    
    // 5. 总输出计算和限幅（用fminf/fmaxf安全限幅）
    total_out = p_out + i_out + d_out;
    total_out = fminf(total_out, pid->Output_Max);  // 上限限制
    total_out = fmaxf(total_out, pid->Output_Min);  // 下限限制
    
    // 6. 更新上一次误差
    pid->Error_Prev = pid->Error;
    
    return total_out;
}

/**
 * @brief  重置PID状态
 */
static void WHT_PID_Float_Reset(WHT_PID_Float_Ctrl_t *pid)
{
    if (pid == NULL) return;
    
    pid->Error = 0.0f;
    pid->Error_Prev = 0.0f;
    pid->Error_Sum = 0.0f;
    pid->Derivative_Filtered = 0.0f;
    pid->Feedback = 0.0f;
}

/**
 * @brief  配置自适应调参
 */
static void WHT_PID_Float_Config_Adaptive(WHT_PID_Float_Ctrl_t *pid, bool enable, float kp_min, float kp_max, float ki_min, float ki_max, float kd_min, float kd_max, float err_large, float err_med, float diff_small)
{
    if (pid == NULL) return;
    
    // 1. 配置自适应使能和系数范围
    pid->Adaptive_En = enable;
    pid->kp_min = kp_min < 0.0f ? 0.0f : kp_min;                  // kp最小≥0
    pid->kp_max = kp_max < pid->kp_min ? pid->kp_min : kp_max;    // kp最大≥最小
    pid->ki_min = ki_min < 0.0f ? 0.0f : ki_min;                  // ki最小≥0
    pid->ki_max = ki_max < pid->ki_min ? pid->ki_min : ki_max;    // ki最大≥最小
    pid->kd_min = kd_min < 0.0f ? 0.0f : kd_min;                  // kd最小≥0
    pid->kd_max = kd_max < pid->kd_min ? pid->kd_min : kd_max;    // kd最大≥最小
    
    // 2. 配置误差阈值（确保阈值合法：大>中>0，变化率阈值>0）
    pid->Adaptive_Err_Large = err_large < err_med ? err_med : err_large;  // 大阈值≥中阈值
    pid->Adaptive_Err_Med = err_med < 0.0f ? 0.0f : err_med;              // 中阈值≥0
    pid->Adaptive_Diff_Small = diff_small < 0.0f ? 0.0f : diff_small;     // 变化率阈值≥0
}

/* PID函数表初始化 */
const WHT_PID_Float_t WHT_PID_Float =
{
    .WHT_Init                     = WHT_PID_Float_Init,
    .WHT_Reset                    = WHT_PID_Float_Reset,
    .WHT_Config_Integral_Limit    = WHT_PID_Float_Config_Integral_Limit,
    .WHT_Config_Derivative_Filter = WHT_PID_Float_Config_Derivative_Filter,
    .WHT_Set_Setpoint             = WHT_PID_Float_Set_Setpoint,
    .WHT_Compute                  = WHT_PID_Float_Compute,
    .WHT_Config_Adaptive          = WHT_PID_Float_Config_Adaptive,
};
